#ifndef _SPARSE_VOXEL_OCTREE_H
//#include "SegmentedGrid.h"
#include "volume3DHash.h"
#include "octNode.h"
#include <stack>

#define TOPO
#ifdef TOPO
#include "SegmentedGrid.h"

#endif
#include <math.h>


/*
SVO class basic concept
Level 0 -  is the leaf level
domain is define by the two corner of the AABB
each node in the octree is define an domain, it determined where to put this node
the root have a domain that contain the whole SVO space.

*/
template<typename T>
class sparseVoxelOctree
{
public:
	sparseVoxelOctree(int blockSize = 16);
	~sparseVoxelOctree();	
	//OV = octreeVolume
	//SVO = sparse voxel octree

#ifdef TOPO	
	unsigned int createOVFromSegmentedGrid(SegmentedGrid *, int blockSize);
#endif

	bool createOVFromHashVolume(volume3DHash<T> *volHash, int blockPoolDim=0);
	//void topDownNodeInsertion(octNode<T> *node,

	octNode<T>* getRoot(){return _root;}

	//
	void initVolumeDomain(int minZ, int minY, int minX, int maxZ, int maxY, int maxX);

	//search and update
	void DFS();


protected:
	//helper function
	int _calculateLevel(int NodeDim);
	//need the range to locate where to put the node
	//TODO add the homogeneous block option
	void _addNode(volume3D<T> *vol, int minZ, int minY, int minX, int maxZ, int maxY, int maxX);

protected:
	int _dim;// virtual dimension

	////////////////
	volume3DHash<T> *_volHash;

	///////////////
	int _blockSize;
	int _blockPoolDim;
	int _blockNum, _nodeNum;

	//////////////
	octNode<T> *_root;
	

};

//////////////////////
template<typename T>
sparseVoxelOctree<T>::sparseVoxelOctree(int blockSize)
	:_root(NULL),
	_blockNum(0),
	_nodeNum(0),
	_blockSize(blockSize)	
{
	//init _blockPoolDim assume you have 500MB pool in GPU TODO
	_blockPoolDim = 512/blockSize;

	//////////////////////TEST MEMBER
	//int v1 = _calculateLevel(32);
	//int v2 = _calculateLevel(32);
	//int v3 = _calculateLevel(1024);




}
template<typename T>
sparseVoxelOctree<T>::~sparseVoxelOctree()
{
}


#ifdef TOPO
template<typename T>
unsigned int  sparseVoxelOctree<T>::createOVFromSegmentedGrid(SegmentedGrid *sGrid, int blockSize)
{
	_blockSize = blockSize;
	unsigned int featureCount = 0;
	//create Volume Hash
	cout<<"start hashing volume ==========START ALL"<<endl;
	printf("Dim: %d %d %d \n", sGrid->mDimension[2], sGrid->mDimension[1], sGrid->mDimension[0]);
	volume3DHash<unsigned int> volHash(sGrid->mDimension[2],
															sGrid->mDimension[1],
															sGrid->mDimension[0],0,blockSize);
	sGrid->setThreshold(5.0);
	for(sGrid->mIte = sGrid->mHierarchy.beginActive(); 
			sGrid->mIte != sGrid->mHierarchy.endActive(); sGrid->mIte++) 
	{
		unsigned int ID = sGrid->mIte->id();
		SegmentedGrid::Segment *seg = &sGrid->mSegments[sGrid->mHierarchy.mappedIndex(ID)];
		int featureSize = seg->elements.size();
		featureCount++;
		//cout<<"feature ID: "<<ID<<" size:"<<featureSize<<endl;
		for(int i=0; i<featureSize; i++)
		{
			SegmentedGrid::Cell *voxel = &seg->elements[i];
			//if(sGrid->mIte->agent()->id() == ID && sGrid->mIte->merged(sGrid->mHierarchy.parameter()))
			//	printf("==|\n");
			//volHash.addVoxel(voxel->pos[2], voxel->pos[1], voxel->pos[0], sGrid->mIte->agent()->id());
			volHash.addVoxel(voxel->pos[2], voxel->pos[1], voxel->pos[0], sGrid->mIte->agent()->id());
			//volHash.addVoxel(voxel->pos[2], voxel->pos[1], voxel->pos[0],ID);
			//volHash.addVoxel(voxel->pos[2], voxel->pos[1], voxel->pos[0], 50000);
		}

	}

	cout<<"occupiedBlocks: "<<volHash.occupiedBlocks()<<" Feature Count:"<<featureCount<<endl;
	printf("finish hashing volume ==========\n");

	//volHash.writeToFile("test-comb","./");

	bool isOK ;
	cout<<"start building tree! ======="<<endl;
	isOK = createOVFromHashVolume(&volHash, blockSize);
	cout<<"end building tree! =======DONE ALL" <<endl;

	volHash.convertToRawVolume();
	volHash.writeToFile("test-comb-singleID","./");

	return featureCount;
}
#endif
/////////////////////////////////////////////////////////////////

template <typename T>
bool sparseVoxelOctree<T>::createOVFromHashVolume(volume3DHash<T> *vol, int blockPoolDim)
{
	
	_volHash = vol;
	_blockSize = vol->getBlockSize();

	initVolumeDomain(0,0,0,vol->getDimZ(), vol->getDimY(), vol->getDimX());
	int count = 0;

	for (int k=0; k<vol->getBlockDimZ(); k++)
		for (int j=0; j<vol->getBlockDimY(); j++)
			for (int i=0; i<vol->getBlockDimX(); i++)
			{
				T* buffer = vol->getBlockRegularGridData(k,j,i);

				if(buffer)
				{
					count++;
					volume3D<T>* subVol = new volume3D<T>(buffer,_blockSize,_blockSize,_blockSize);
					//find the octNode level and domain
					_addNode(subVol, k*_blockSize, j*_blockSize, i*_blockSize,
																			(k+1)*_blockSize, (j+1)*_blockSize, (i+1)*_blockSize);
				}
			}

	//printf("Allocated buffer %d\n", count);

	return true;
}

template <typename T>
void sparseVoxelOctree<T>::initVolumeDomain(int minZ, int minY, int minX, int maxZ, int maxY, int maxX)
{
	int x = maxX-minX;
	int y = maxY-minY;
	int z = maxZ-minZ;

	int maxDim = max(x,max(y,z));
	//get the smallest 2^x bigger than maxDim
	_dim = 1;
	while(_dim<maxDim)
		_dim<<=1;
}

////////////////////////
template <typename T>
int sparseVoxelOctree<T>::_calculateLevel(int NodeDim)
{
	int i=0;
	while (NodeDim*int(pow(2.0, i))< _dim)
	{
		i++;
	}
	return i;
}

template <typename T>
void sparseVoxelOctree<T>::_addNode(volume3D<T> *vol, int minZ, int minY, int minX, int maxZ, int maxY, int maxX)
{
	int level = _calculateLevel(maxX-minX);
	//printf("Target: (%d %d %d - %d %d %d)\n",minZ,minY,minX,maxZ, maxY, maxZ);
	//normalized center of the node
	float cZ, cY, cX;
	cZ = (maxZ+minZ)*0.5f/float(_dim);
	cY = (maxY+minY)*0.5f/float(_dim);
	cX = (maxX+minX)*0.5f/float(_dim);

	if(!_root)
	{
		_root = new octNode<T>(0,0,0, _blockSize, 0, 0, 0, _dim, _dim, _dim,//the block domain range 
			_calculateLevel( _dim), NULL);
	}

	//node->setLevel(level);
	octNode<T> *currentNode = _root ;
	//update domain to the entire volume
	minZ = 0; maxZ = _dim;
	minY = 0; maxY = _dim;
	minX = 0; maxX = _dim;

//printf("Called AddNode()\n");
	
	//which children
	int childrenIndexX, childrenIndexY, childrenIndexZ;
	for(int i=1; i<=level; i++)
	{
		assert(cX<1.0f && cY<1.0f && cZ<1.0f);
		//printf("center: %f %f %f\n",cZ,cY,cX);

		childrenIndexZ = int((float(cZ)*2.0f)); 
		childrenIndexY = int((float(cY)*2.0f)); 
		childrenIndexX = int((float(cX)*2.0f)); 

		cZ = cZ*2.0f - float(childrenIndexZ);
		cY = cY*2.0f  - float(childrenIndexY);
		cX = cX*2.0f  - float(childrenIndexX);
		
		//update domain
		int dZ = maxZ - minZ;
		minZ += childrenIndexZ*dZ/2; 
		maxZ = minZ + dZ/2;

		int dY = maxY - minY;
		minY += childrenIndexY*dY/2;
		maxY = minY + dY/2;

		int dX = maxX - minX;
		minX += childrenIndexX*dX/2;
		maxX = minX + dX/2;
		//printf("MinMax: (%d %d %d - %d %d %d)\n",minZ,minY,minX,maxZ, maxY, maxZ);

		if(!currentNode->getChild(childrenIndexZ,childrenIndexY,childrenIndexX))
		{
			//add children
			octNode<T>* node = new octNode<T>(childrenIndexZ, childrenIndexY, childrenIndexX,
															_blockSize, minZ, minY, minX, maxZ, maxY, maxX, level, currentNode);
			//printf("create New Node!\n");
			//printf("Index: %d %d %d ",childrenIndexZ,childrenIndexY,childrenIndexX);
			currentNode->addChild(childrenIndexZ,childrenIndexY,childrenIndexX, node);
		}
		currentNode = currentNode->getChild(childrenIndexZ,childrenIndexY,childrenIndexX);

		//set the volume to the right node
		if(i==level)
			currentNode->setVolume(vol);
	}
}


template <typename T>
void sparseVoxelOctree<T>::DFS()
{
	std::stack<octNode<T>*> nodeStack;
	nodeStack.push(_root);
	octNode<T> *currentNode = 0;
	_nodeNum = 0;
	_blockNum = 0;

	while(!nodeStack.empty())
	{
		currentNode = nodeStack.top();
		nodeStack.pop();
		if(currentNode)
		{
			for(int k=0; k<2; k++)
				for(int j=0; j<2; j++)
					for(int i=0; i<2; i++)
					{
						if(currentNode->getChild(k,j,i))
							nodeStack.push(currentNode->getChild(k,j,i));
					}
			//update stuff here
			_nodeNum++;
			if(currentNode->getVolume())
				_blockNum++;

		}		


	}
	printf("Node Number: %d\n",_nodeNum);
	printf("Block Number: %d\n",_blockNum);


}


#endif